/*
 * You need libhighgui-dev libcv-dev opencv.. and probably som other stuff for this to build..
 * incs located in /usr/include/opencv and some stuff in /usr/lib/ ...... isch....
 * //Mårten 7/11-11
 *
 *Uncomment SENDDATA to allow send angle and position to controller. //Victor
 */

#include "cv.h"
#include "highgui.h"
#include "math.h"
#include <pthread.h>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <unistd.h>
#include "beamData.h"
#include <sys/time.h>

using namespace std;

#define SENDDATA
#define CAMSETUP
//#define TRUSTWEBCAM
#define FRONTCAMERA

//ROI of visual reference
#define ROIWIDTH 640
#define ROIHEIGHT 60

#define MIN_REF_CHANGE 5
#define MAX_DIF_VERTICAL 10

//Scale factors
#define MAX_ANGLE 10          //MAX_ANGLE corresponds to PI/2
/*
 * Template tracking
 * parameters
 *
 */
//#define CIRCLE
#define TEMPLATE

#define  TPL_WIDTH       18      /* template width       */
#define  TPL_HEIGHT      18      /* template height      */
#define  WINDOW_WIDTH    160      /* search window width  */
#define  WINDOW_HEIGHT   60      /* search window height */
#define  THRESHOLD       0.3

IplImage *frame, *tpl, *tm;
int object_x0, object_y0, is_tracking = 0;
/*
 * END of template tracking parameters
 */

//------Variables--------
double reference = 0.0;

int thresh = 1;
IplImage* img = 0;
IplImage* img0 = 0;
IplImage* imgBG = 0;
CvMemStorage* fstorage = 0;
int c;
double angle;
int xPos;
double xPosDouble;
int xPosOld = 0;
int px[0], py[0];
int edge_thresh;
IplImage *src = NULL;
IplImage *gray = NULL;
IplImage *edge = NULL;
IplImage *edgeBG = NULL;
IplImage* refROI = NULL;
IplImage s;
//IplImage *subpic;
//CvMemStorage* cstorage;
//int posX;
int posY;
bool once;
CvCapture* capture;
CvRect black;
//TCP
int sockfd, newsockfd, portno;
int n;
char buffer[256];
string host;
socklen_t clilen;
struct sockaddr_in serv_addr, cli_addr;
cv::Mat *background;
cv::Mat *background_square;
cv::Mat *imgdiv;

//Data from Matlab code--> possibly changed to c++ code.
//Arrays of size 640 with beam description.
double *angleData;
double *beamData;
double *tangentData;

//-----
//-----------------------

/*
 * Template Tracking functions
 */

/* mouse handler */
void mouseHandler(int event, int x, int y, int flags, void *param) {
	/* user clicked the image, save subimage as template */
	if (event == CV_EVENT_LBUTTONUP) {
		object_x0 = x - (TPL_WIDTH / 2);
		object_y0 = y - (TPL_HEIGHT / 2);

		cvSetImageROI(src,
				cvRect(object_x0, object_y0, TPL_WIDTH, TPL_HEIGHT ));
		cvCopy(src, tpl, NULL );
		cvResetImageROI(src);

		/* template is available, start tracking! */
		fprintf(stdout, "Template selected. Start tracking... \n");
		is_tracking = 1;
	}
}



/*
 * TODO: please, split up the code in different files!!!
 * END of Template finding methods
 */

/*
 * Reference get and set funct320ions
 *
 */
double getReference() {
	return reference;
}

void setReference(double newRef) {
	reference = newRef;
}

//To get an image with just yellow as white and everything else as black.
IplImage* GetThresholdedImage(IplImage* img) {
	IplImage* imgHSV = cvCreateImage(cvGetSize(img), 8, 3);
	cvCvtColor(img, imgHSV, CV_BGR2HSV);
	IplImage* imgThreshed = cvCreateImage(cvGetSize(img), 8, 1);
	cvInRangeS(imgHSV, cvScalar(0, 20, 180), cvScalar(18, 250, 250),
			imgThreshed);
	//cvInRangeS(imgHSV, cvScalar(0, 10, 200), cvScalar(40, 255, 255), imgThreshed);
	cvReleaseImage(&imgHSV);
	return imgThreshed;
}

void initCam() {

	edge_thresh = 100;
	src = 0;
	gray = cvCreateImage(cvSize(640, 480), 8, 1);
	edge = cvCreateImage(cvSize(640, 480), 8, 1);
//	cstorage = cvCreateMemStorage(0);
	fstorage = cvCreateMemStorage(0);

	background = new cv::Mat(cvSize(640, 480), CV_32FC1);
	*background *= 0;
	background_square = new cv::Mat(cvSize(640, 480), CV_32FC1);
	*background_square *= 0;
	imgdiv = new cv::Mat(cvSize(640, 480), CV_32FC1);

	//Colourdetectiontesting
	black = cvRect(0, 0, 640, 200);
	//----------------------

	//Only used for test purposes:
	once = true;
	//---------------------------

	/*
	 * #define TRUSTWEBCAM 1
	 * #define FRONTCAMERA 0
	 *
	 */
#ifdef FRONTCAMERA
	cout << "Using front camera (outside the lab)" << endl;
	capture = cvCaptureFromCAM(0);
#elif defined TRUSTWEBCAM
	cout << "Using trustwebcam camera (To work with the camera in the lab)"
	<< endl;
	capture = cvCaptureFromCAM(1);
#endif
}
/*
 * Method to send error messages to stderr
 * and then exit the program
 */
void error(const char *msg) {
	perror(msg);
	exit(1);
}
void initWindows() {
	//	Removed for anti bloating purposes
	//cvNamedWindow("src", 1);
	//cvNamedWindow("gray", 2);
	//cvNamedWindow("edge", 3);
	//cvNamedWindow("colourdetect", 4);

	//cvNamedWindow("subtracted", 5);
	cvNamedWindow("CameraSetUp", 6);
	//cvNamedWindow("Reference", 7);
}
void initTCP() {
	//
	//TCP communication
	//
	portno = 4444;
	host = "127.0.0.1";

	sockfd = socket(AF_INET, SOCK_STREAM, 0);
	if (sockfd < 0)
		error("ERROR opening socket");
	bzero((char *) &serv_addr, sizeof(serv_addr));
	serv_addr.sin_family = AF_INET;
	serv_addr.sin_addr.s_addr = INADDR_ANY;
	serv_addr.sin_port = htons(portno);
	if (bind(sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0)
		error("ERROR on binding socket");
	listen(sockfd, 5);
	clilen = sizeof(cli_addr);

	//
	//TCP init end
	//

	//wait for socket to be connected
	cout << "Waiting for tcp connection. Please start java GUI." << endl;
	newsockfd = accept(sockfd, (struct sockaddr *) &cli_addr, &clilen);
	if (newsockfd < 0)
		error("ERROR on accept");
	cout << "TCP connection up. " << "Client port is: "
			<< ntohs(cli_addr.sin_port) << endl;
}

void close() {
	cvReleaseCapture(&capture);
	close(newsockfd);
	close(sockfd);
	cvDestroyAllWindows();
}

/*
 * Method to send the reference angle just to be able to
 * control the beam when beam is in REF mode. The modes
 * of the controller is set in the Java GUI usin the
 * radio buttons.
 * Don't forget to set the "sendToProcess" constant to
 * true if you do want to send the data to the process
 * (It's on line 20 in Regul.java in the Lab1 folder)
 *
 */
void sendAngleRef(double angleRef) {
	string tmp;
	stringstream sstm;
	sstm << "angleRef " << angleRef << "\r\n";
	tmp = sstm.str();
	//	cout << tmp.c_str();
#ifdef SENDDATA
	n = write(newsockfd, (void*) tmp.c_str(), tmp.size());
	if (n < 0)
		error("ERROR writing angleRef to socket");
#endif
}

/*
 * Method to send the angle of the beam to the controller
 *
 */
void sendAngle(double angle) {
	string tmp;
	stringstream sstm;
	sstm << "angle " << angle << "\r\n";
	tmp = sstm.str();
	//	cout << tmp.c_str();
#ifdef SENDDATA
	n = write(newsockfd, (void*) tmp.c_str(), tmp.size());
	if (n < 0)
		error("ERROR writing a320ngle to socket");
#endif
}

/*
 * Method to send the x position of the ball to the controller
 *
 */
void sendPos(double posX) {
	string tmp;
	stringstream sstm;
	sstm << "pos " << posX << "\r\n";
	tmp = sstm.str();
#ifdef SENDDATA
	n = write(newsockfd, (void*) tmp.c_str(), tmp.size());
	if (n < 0)
		error("ERROR writing pos to socket");
#endif
}
void sendPosRef(double posRef) {
	string tmp;
	stringstream sstm;
	sstm << "posRef " << posRef << "\r\n";
	tmp = sstm.str();
#ifdef SENDDATA
	n = write(newsockfd, (void*) tmp.c_str(), tmp.size());
	if (n < 0)
		error("ERROR writing posRef to socket");
#endif
}

/* track object */
void trackObject() {
	CvPoint minloc, maxloc;
	double minval, maxval;

	/* setup position of search window */
	int win_x0 = object_x0 - ((WINDOW_WIDTH - TPL_WIDTH) / 2);
	int win_y0 = object_y0 - ((WINDOW_HEIGHT - TPL_HEIGHT) / 2);

	/*
	 * Ooops, some bugs here.
	 * If the search window exceed the frame boundaries,
	 * it will trigger errors.
	 *
	 * Add some code to make sure that the search window
	 * is still within the frame.
	 */

	/* search object in search window */
	if(win_x0+WINDOW_WIDTH>=630)
		win_x0 = 630-WINDOW_WIDTH;
	else if(win_x0<=10)
		win_x0 = 10;
	cvSetImageROI(src, cvRect(win_x0, win_y0, WINDOW_WIDTH, WINDOW_HEIGHT ));
	cvMatchTemplate(src, tpl, tm, CV_TM_SQDIFF_NORMED);
	cvMinMaxLoc(tm, &minval, &maxval, &minloc, &maxloc, 0);
	cvResetImageROI(src);
	/* if object found... */
	if (minval <= THRESHOLD) {
		/* save object's current location */
		object_x0 = win_x0 + minloc.x;
		object_y0 = win_y0 + minloc.y;

		/* and draw a box there */
		cvRectangle(src, cvPoint(object_x0, object_y0), cvPoint(object_x0
				+ TPL_WIDTH, object_y0 + TPL_HEIGHT ), cvScalar(0, 0, 255, 0),
				1, 0, 0);
		xPos = cvRound(object_x0+TPL_WIDTH/2);
		xPos -= 640/2;
		xPosDouble = ((double)xPos)/320.0*10.0;
		if(!isnan(xPos)) {
			sendPos(xPosDouble);
			//sendPosRef(0);
			xPosOld = xPos;
		}

	} else {
		/* if not found... */
		fprintf(stdout, "Lost object.\n");
		is_tracking = 0;
	}
}

/*
 * Camera calibration should be done here.
 * 1 - Position of beam and camera (beam inside the rectangle)
 * 2 - Code for background finding, moving the beam and collecting images
 * to calculate the median / mean.
 * 3 - Center of rotation
 * 4 - Other stuff...
 *
 */
void camSetUp() {
#ifdef CAMSETUP

	/*
	 * Template init
	 */
	/* create template image */
	src = cvQueryFrame(capture);
	tpl = cvCreateImage(cvSize(TPL_WIDTH, TPL_HEIGHT ), src->depth,
			src->nChannels);

	/* create image for template matching result */
	tm = cvCreateImage(cvSize(WINDOW_WIDTH - TPL_WIDTH + 1, WINDOW_HEIGHT
			- TPL_HEIGHT + 1), IPL_DEPTH_32F, 1);

	cvSetMouseCallback("CameraSetUp", mouseHandler, NULL );

	/*
	 * Template init END
	 */

	//1 - Position of beam and camera (beam inside the rectangle)
	cout << "CameraSetUp" << endl
			<< "Correct position and distance between camera and beam:" << endl;
	cout << "Press any key when the camera is correctly placed." << endl;

	int key = 0;
	while (cvWaitKey(10) < 0) { //key != 'o') {
		src = cvQueryFrame(capture);

		cvRectangle(src, cvPoint(10, 150), cvPoint(630, 250),
				CV_RGB( 255, 0, 0 ), 1, 8, 0);
		cvLine(src, cvPoint(320, 180), cvPoint(320, 220), CV_RGB( 0, 255, 0 ),
				2, 8, 0);
		cvShowImage("CameraSetUp", src);
		//key = cvWaitKey(10);
		//cout << key<< endl;
	}

	//2 - Code for background finding, moving the beam and collecting images
	cout << "Calculating background and center of rotation" << endl;
	//initial angle
	sendAngleRef(-10.0);
	int nbrOfFrames = 100;
	double stepSize = 18.0 / (nbrOfFrames - 1);
	usleep(1000000);
	src = cvQueryFrame(capture);
	for (int i = 0; i < nbrOfFrames; i++) {
		src = cvQueryFrame(capture);
		cvShowImage("CameraSetUp", src);
		cvCvtColor(src, gray, CV_BGR2GRAY);
		gray->origin = 1;

		cvSmooth(gray, gray, CV_GAUSSIAN, 7, 7);
		cv::Mat imgMat(gray);
		imgMat.convertTo(imgMat, CV_32FC1, 1 / 255.0, 0);
		cv::accumulate(imgMat, *background);
		//rotate the beam and wait
		sendAngleRef((double) (-9.0) + stepSize * i);
		//		cout << "[circledetect]: angle set to:" <<(double)(-9.0)+stepSize*i << endl ;
		usleep(1000000 / nbrOfFrames);
	}
	*background /= nbrOfFrames;
	cout << "Background found. Press any key to continue." << endl;
	sendAngleRef(0.0);
	key = 0;
	key = cvWaitKey();

	//Anything else before camThread starts???

#else
	cout << "Camera set up OFF" << endl;
#endif

}

/*
 * Thread that runs the camera code
 *
 * TODO: This thread has some memory leakage as it creates new
 * matrices and some other stuff every time it runs. Fix this
 * to get rid of problems with "insufficient memory".
 * //Mårten
 *
 * Around 10 ms to get a frame from cvQueryFrame
 * Around 60 ms to find ball/beam and remove bg
 * Socket communication time is neglectable.
 * 13ms ball detection
 * 10ms beam detection
 *
 */
void* camThread(void*) {

	int frame = 0;
//	double tmpD = 0;
	IplImage* imgBG;
	double x = 0, y = 0;
	int counter=0;

	//-----Matrices used in background subtraction------
	cv::Mat imgMat;
	cv::Mat subpic(cvSize(640, 480), CV_32FC1);
	cv::Mat res(cvSize(640, 480), CV_8UC1);
	//------------------------------------------
	//Matrices used in edge, line and circle detection----
	cv::Mat circleD;
	cv::Mat edgeMat;
	cv::Mat src_s;
	vector<cv::Vec3f> circles_bg;
	vector<cv::Vec4i> lines_bg;
	//---------------------------------------------
	//Stuff used in reference finding-----------
	cv::Mat refMat;
	vector<cv::Vec4i> ref_line;
	cv::Vec4i l;

	//-----------------------------------------

    timeval time1, time2;
    double elapsedTime;

	for (;;) {


		src = cvQueryFrame(capture);




		cvCvtColor(src, gray, CV_BGR2GRAY);
		gray->origin = 1;

		cvSmooth(gray, gray, CV_GAUSSIAN, 7, 7);
		imgMat = gray;
		imgMat.convertTo(imgMat, CV_32FC1, 1 / 255.0, 0);

		//----
		//---START BACKGROUND SUB---
		// work for the moment best with the black ball/Kicki
		cv::absdiff(imgMat, *background, subpic);
		// wingat number, threshold..
		cv::compare(subpic, 0.12, res, cv::CMP_GT);// 0.09, res, cv::CMP_GT);
		//In gray scale
		res.convertTo(res, CV_32FC1, 1 / 255.0);
		//Removing background
		res = imgMat - (-res + 1);
		res.convertTo(res, CV_8UC1, 255.0);
		s = res;
		//
		//---END BACKGROUND SUB---
		//


		imgBG = GetThresholdedImage(src);
		cvSmooth(imgBG, imgBG, CV_GAUSSIAN, 7, 7);
		//cvCanny(imgBG, edgeBG, (float)edge_thresh, (float)edge_thresh*3, 5);//Runs internally HougheCircles


		//Find Line
		//ROI
		//cvSetImageROI(&s, cvRect(10, 100, 630, 450));
		edgeBG = cvCloneImage(&s);
		//cvResetImageROI(&s);
		//endROI
		circleD=edgeBG;
		cvCanny(edgeBG, edgeBG, 20, 100, 3);
		edgeMat=edgeBG;
		src_s=src;

#ifdef CIRCLE
		//Find circles
//		cv::HoughCircles(circleD, circles_bg, CV_HOUGH_GRADIENT, 2, 80, 140, 25, 10, 13);
		cv::HoughCircles(circleD, circles_bg, CV_HOUGH_GRADIENT, 2, 640, 50, 50, 10, 15);

		//Drawing circles
		//		printf("found %d circles, %d lines\n", circles_bg.size(), lines_bg.size());

		// TODO: Need to calculate the x position in relationship to the beams center
		// of rotation and send it instead of the x position in relation to the window
		// coordinates.
		//cv::Mat src_s(src);
		xPos = 0;
		for (size_t i = 0; i < circles_bg.size(); i++) {
			xPos = cvRound(circles_bg[i][0]);
			//if(xPos>640/2)
			xPos -= 640/2;
			//else
			//xPos = 640/2-xPos;
//			if( !( xPos<xPosOld-100 || xPos>xPosOld+100 )) {
				cv::Point center(cvRound(circles_bg[i][0]), cvRound(circles_bg[i][1]));
				int radius = cvRound(circles_bg[i][2]);
				cv::circle(src_s, center, 3, cv::Scalar(0, 255, 0), -1, 8, 0);
				cv::circle(src_s, center, radius, cv::Scalar(0, 255, 0), 3, 8, 0);
				//TODO: sendPos(X) where X is the center of the ball
				//				cout << "[circledetect]: radius = " << radius << endl;
				xPosDouble = ((double)xPos)/310.0*10.0;
				if(!isnan(xPos)) {
					sendPos(xPosDouble);
					sendPosRef(0);
					xPosOld = xPos;
				}
//			}
//			else
//			cout << "probably not the correct circle detected at xPos: " << xPos << endl;
		}
		//		sendPos(0);
		//		sendPosRef(0);
#elif defined TEMPLATE

		/* perform tracking if template is available */
		if (is_tracking)
			trackObject();

#endif




	    gettimeofday(&time1, NULL);
		//Find lines
		cv::HoughLinesP(edgeMat, lines_bg, 1, CV_PI / 180, 120, 450, 150);
		//
		//Drawing Lines and computing angles.
		//I'm not quite certain that this works
		//as intended, it would be great if
		//someone else takes a look at it and
		//evaluates the computations
		//
		angle = 0;
		x = y = 0.0;
		double *angleArray = new double[lines_bg.size()];
		double mean = 0;
		for (size_t i = 0; i < lines_bg.size(); i++) {
			cv::Point p1(lines_bg[i][0], lines_bg[i][1]);
			cv::Point p2(lines_bg[i][2], lines_bg[i][3]);
			cv::line(src_s, p1, p2, cv::Scalar(255, 0, 0), 2, 8);
			x = lines_bg[i][2] - lines_bg[i][0];
			y = lines_bg[i][3] - lines_bg[i][1];
			if (x > 0 && y > 0) {
				angle = -(atan(y / x) / M_PI_4) * MAX_ANGLE;
			} else if (y < 0) {
				y = abs(y);
				angle = (atan(y / x) / M_PI_4) * MAX_ANGLE;
			}
			angleArray[i] = angle;
			mean += angle;

		}
		mean = mean / ((double) lines_bg.size());
		if (!isnan(mean))
			sendAngle(mean);

	    gettimeofday(&time2, NULL);
	    elapsedTime = (time2.tv_usec - time1.tv_usec) / 1000.0;
		//cout << elapsedTime << endl;
		/*
		 * Visual reference
		 * Change the reference if current_ref - pos_x of detected line in refROI
		 * is larger than MIN_REF_CHANGE (currently set up to 5 pixels), and
		 * is a vertical line MAX_DIF_VERTICAL
		 * initial value for reference is 320 (in pixels)
		 */
		cvRectangle(src, cvPoint(5, 5),
				cvPoint(ROIWIDTH - 1 - 10, ROIHEIGHT + 10), CV_RGB( 0, 255, 0 ),
				2, 8, 0);
		cvSetImageROI(&s, cvRect(10, 10, ROIWIDTH - 10, ROIHEIGHT + 10));
		refROI = cvCloneImage(&s);
		cvResetImageROI(&s);

		cvCanny(refROI, refROI, 20, 100, 3);
		refMat=refROI;
		cv::HoughLinesP(refMat, ref_line, 1, CV_PI / 180, 20, 30, 3);
		//Drawing Lines only if vertical
		for (size_t i = 0; i < ref_line.size(); i++) {
			l = ref_line[i];

			//Change reference
			if (abs(l[0] - l[2]) <= MAX_DIF_VERTICAL) {
				cv::line(src_s, cv::Point(l[0], l[1]), cv::Point(l[2], l[3]),
						cv::Scalar(255, 0, 255), 5, CV_AA);
				int diff = abs(round((l[0] + l[2]) / 2) - getReference());
				if (diff > MIN_REF_CHANGE) {
					double currentRef = ((double)(round((l[0] + l[2]) / 2) )- 320)/310.0*10.0;
					setReference(currentRef);
					sendPosRef(currentRef);
					cout << "[circledetect]: New reference: " << getReference() << endl;
				}
			}
		}
		//End of finding vertical line for reference.

		//Drawing the images

		//		Removed theese as the screen gets bloated otherwise
		//		cvShowImage("colourdetect", imgBG);
		//		cvShowImage("gray", gray);
//		if(counter == 10){
			//cvShowImage("edge", edgeBG);

			//cvShowImage("subtracted", &s);
			cvShowImage("CameraSetUp", src);
			//cvShowImage("Reference", refROI);
			counter = 0;
//		}
//		else
//			counter++;
			cvReleaseImage( &imgBG );
			//cvReleaseImage( s);

			//cvReleaseImage(&src);
		//	cvReleaseImage(&gray);
			cvReleaseImage(&edge);
			cvReleaseImage(&edgeBG);
			cvReleaseImage(&refROI);

		cvClearMemStorage(fstorage);



		c = cvWaitKey(10);
		if (c == 27)
			break;
	}
	close();
	return 0;
}

void* TCPThread(void*) {
	/*	newsockfd = accept(sockfd, (struct sockaddr *) &cli_addr, &clilen);
	 if (newsockfd < 0)
	 error("ERROR on accept");
	 cout << "TCP connection up. " << "Client port is: "
	 << ntohs(cli_addr.sin_port) << endl;
	 *///cout << ntohs(serv_addr.sin_port) << endl;
	for (;;) {
		bzero(buffer, 256);
		n = read(newsockfd, buffer, 255);
		if (n < 0)
			error("ERROR reading from socket");
		printf("Here is the message: %s\n", buffer);
		n = write(newsockfd, "I got your message", 18);
		/*if (n < 0)
		 error("ERROR writing to socket");
		 if (strcmp(buffer, "getAngle") == 0)
		 sendAngle(angle);
		 if (strcmp(buffer, "getPos") == 0)
		 sendPos();

		 */
	}
	return 0;
}

int main(int argc, char** argv) {

	/*
	 * Retrieve beam data from .txt files
	 * Changed to c++ if it works.
	 *//*
	 angleData = getAngleData();
	 beamData = getBeamData();
	 tangentData = getTangentData();
	 */
	initTCP();
	initCam();
	initWindows();

	camSetUp();

	pthread_t camthread;
	pthread_t tcpthread;

	void* (*camThreadPtr)(void*);
	camThreadPtr = camThread;
	void* (*TCPThreadPtr)(void*);
	TCPThreadPtr = TCPThread;
	//---- Start Threads -------
	//	pthread_create(&tcpthread, NULL, TCPThreadPtr, NULL);
	pthread_create(&camthread, NULL, camThreadPtr, NULL);
	//-------------------------
	//	pthread_join(tcpthread, NULL);
	pthread_join(camthread, NULL);


	close();
	return 1;
}
